#include <mathematics/Node.h>

namespace ne
{
    Node::Node()
        : mInitialPosition(Vector3::ZERO),
          mInitialOrientation(Quaternion::IDENTITY),
          mInitialScale(Vector3::ONE),
          mPosition(Vector3::ZERO),
          mOrientation(Quaternion::IDENTITY),
          mScale(Vector3::ONE),
          mDerivedPosition(Vector3::ZERO),
          mDerivedOrientation(Quaternion::IDENTITY),
          mDerivedScale(Vector3::ONE),
          mTransformMatrix(Matrix4::IDENTITY),
          mInverseTransformMatrix(Matrix4::IDENTITY),
          mNeedUpdate(false),
          mNeedAllChildrenUpdate(false),
          mParentNotified(false),
          mNeedTransformMatrixUpdate(false),
          mNeedInverseTransformMatrixUpdate(false),
          mpParent(0),
          mpRoot(this)
    {
    }

    void Node::setPosition(const Vector3 &v)
    {
        mPosition = v;
        needUpdate();
    }

    const Vector3& Node::getPosition(const TransformSpace ts) const
    {
        switch (ts)
        {
            case TS_PARENT:
                return mPosition;

            case TS_WORLD:
                return _getDerivedPosition();

            default:
                return Vector3::ZERO;
        }
    }

    void Node::setOrientation(const Quaternion &q)
    {
        mOrientation = q;
        needUpdate();
    }

    const Quaternion& Node::getOrientation(const TransformSpace ts) const
    {
        switch (ts)
        {
            case TS_PARENT:
                return mOrientation;

            case TS_WORLD:
                return _getDerivedOrientation();

            default:
                return Quaternion::IDENTITY;
        }
    }

    void Node::setScale(const Vector3 &v)
    {
        mScale = v;
        needUpdate();
    }

    const Vector3& Node::getScale(const TransformSpace ts) const
    {
        switch (ts)
        {
            case TS_PARENT:
                return mScale;

            case TS_WORLD:
                return _getDerivedScale();

            default:
                return Vector3::ONE;
        }
    }

    void Node::setInitial()
    {
        mInitialPosition = mPosition;
        mInitialOrientation = mOrientation;
        mInitialScale = mScale;
    }

    const Vector3& Node::getInitialPosition() const
    {
        return mInitialPosition;
    }

    const Quaternion& Node::getInitialOrientation() const
    {
        return mInitialOrientation;
    }

    const Vector3& Node::getInitialScale() const
    {
        return mInitialScale;
    }

    void Node::translate(const Vector3 &v, const TransformSpace ts)
    {
        switch (ts)
        {
            case TS_LOCAL:
                setPosition(mPosition + mOrientation * v);
                break;

            case TS_PARENT:
                setPosition(mPosition + v);
                break;

            case TS_WORLD:
                if (mpParent)
                {
                    setPosition(mPosition + (mpParent->_getDerivedOrientation().inverse() * v) / mpParent->_getDerivedScale());
                }
                else
                {
                    setPosition(mPosition + v);
                }
                break;

            default:
                break;
        }
    }

    void Node::roll(const real a, const TransformSpace ts)
    {
        rotate(Vector3::UNIT_Z, a, ts);
    }

    void Node::yaw(const real a, const TransformSpace ts)
    {
        rotate(Vector3::UNIT_Y, a, ts);
    }

    void Node::pitch(const real a, const TransformSpace ts)
    {
        rotate(Vector3::UNIT_X, a, ts);
    }

    void Node::rotate(const Vector3 &axis, const real a, const TransformSpace ts)
    {
        Quaternion q(axis, a);
        rotate(q, ts);
    }

    void Node::rotate(const Quaternion &q, const TransformSpace ts)
    {
        Quaternion qnorm = q;
		qnorm.normalize();
        switch (ts)
        {
            case TS_LOCAL:
                setOrientation(mOrientation * qnorm);
                break;

            case TS_PARENT:
                setOrientation(qnorm * mOrientation);
                break;

            case TS_WORLD:
                setOrientation(mOrientation * _getDerivedOrientation().inverse() * qnorm * _getDerivedOrientation());
                break;

            default:
                break;
        }
    }

    void Node::scale(const Vector3 &v)
    {
        setScale(mScale * v);
    }

    Node* Node::getParent() const
    {
        return mpParent;
    }

    Node* Node::getRoot() const
    {
        return mpRoot;
    }

    bool Node::isRoot() const
    {
        if (mpParent)
        {
            return false;
        }
        return true;
    }

    bool Node::isBranch() const
    {
        if (mpParent && !mChildren.empty())
        {
            return true;
        }
        return false;
    }

    bool Node::isLeaf() const
    {
        if (mChildren.empty())
        {
            return true;
        }
        return false;
    }

    bool Node::addChild(Node *child)
    {
        if (child && child != this && mpRoot != child->getRoot())
        {
            NodeSetConstIterator it = mChildren.find(child);
            if (it == mChildren.end())
            {
                Node *pParent = child->getParent();
                if (pParent)
                {
                    pParent->removeChild(child);
                }
                mChildren.insert(child);
                child->_setParent(this);
                return true;
            }
        }
        return false;
    }

    Node* Node::removeChild(Node *child)
    {
        if (child)
        {
            NodeSetIterator it = mChildren.find(child);
            if (it != mChildren.end())
            {
                cancelUpdate(child);
                mChildren.erase(it);
                child->_setParent(0);
            }
        }
        return child;
    }

    void Node::removeAllChildren()
    {
        for (NodeSetIterator it=mChildren.begin(); it!=mChildren.end(); ++it)
        {
            (*it)->_setParent(0);
        }
        mChildren.clear();
        mChildrenToUpdate.clear();
    }

    const NodeSet& Node::getChildren() const
    {
        return mChildren;
    }

    size_t Node::getChildrenCount() const
    {
        return mChildren.size();
    }

    Vector3 Node::convertWorldToLocalVector(const Vector3 &v) const
    {
        _updateFromParent();
        return (mDerivedOrientation.inverse() * v) / mDerivedScale;
    }

    Vector3 Node::convertLocalToWorldVector(const Vector3 &v) const
    {
        _updateFromParent();
        return mDerivedOrientation * (mDerivedScale * v);
    }

    Vector3 Node::convertWorldToLocalPosition(const Vector3 &v) const
    {
        _updateFromParent();
        return (mDerivedOrientation.inverse() * (v - mDerivedPosition)) / mDerivedScale;
    }

    Vector3 Node::convertLocalToWorldPosition(const Vector3 &v) const
    {
        _updateFromParent();
        return mDerivedOrientation * (mDerivedScale * v) + mDerivedPosition;
    }

    Quaternion Node::convertWorldToLocalOrientation(const Quaternion &q) const
    {
        _updateFromParent();
        return mDerivedOrientation.inverse() * q;
    }

    Quaternion Node::convertLocalToWorldOrientation(const Quaternion &q) const
    {
        _updateFromParent();
        return mDerivedOrientation * q;
    }

    void Node::reset()
    {
        setPosition(mInitialPosition);
        setOrientation(mInitialOrientation);
        setScale(mInitialScale);
    }

    void Node::resetPosition()
    {
        setPosition(mInitialPosition);
    }

    void Node::resetOrientation()
    {
        setOrientation(mInitialOrientation);
    }

    void Node::resetScale()
    {
        setScale(mInitialScale);
    }

    void Node::update()
    {
        _update(true);
    }

    void Node::needUpdate(const bool forceParentUpdate)
    {
        mNeedUpdate = true;
        mNeedAllChildrenUpdate = true;
        mNeedTransformMatrixUpdate = true;
        mNeedInverseTransformMatrixUpdate = true;

        if (mpParent && (!mParentNotified || forceParentUpdate))
        {
            mpParent->requestUpdate(this, forceParentUpdate);
            mParentNotified = true;
        }

        // All children will be updated
        mChildrenToUpdate.clear();
    }

    void Node::requestUpdate(Node *child, const bool forceParentUpdate)
    {
        if (!mNeedAllChildrenUpdate && child && mpRoot == child->getRoot())
        {
            mChildrenToUpdate.insert(child);

            if (mpParent && (!mParentNotified || forceParentUpdate))
            {
                mpParent->requestUpdate(this, forceParentUpdate);
                mParentNotified = true;
            }
        }
    }

    void Node::cancelUpdate(Node *child)
    {
        if (child)
        {
            NodeSetIterator it = mChildren.find(child);
            if (it != mChildren.end())
            {
                mChildrenToUpdate.erase(child);
            }
        }

        // Propogate this up if we're done
        if (mChildrenToUpdate.empty() && mpParent && !mNeedAllChildrenUpdate)
        {
            mpParent->cancelUpdate(this);
			mParentNotified = false;
        }
    }

    // Used for convert Local To World
    const Matrix4& Node::getTransformMatrix() const
    {
        _updateTransformMatrix();
        return mTransformMatrix;
    }

    // Used for convert World To Local
    const Matrix4& Node::getInverseTransformMatrix() const
    {
        _updateInverseTransformMatrix();
        return mInverseTransformMatrix;
    }

    void Node::copyTransform(const Node &node, const TransformSpace ts)
    {
        setPosition(node.getPosition(ts));
        setOrientation(node.getOrientation(ts));
        setScale(node.getScale(ts));
    }

    Node::~Node()
    {
        if (mpParent)
        {
            mpParent->removeChild(this);
        }
        removeAllChildren();
    }

    void Node::_setParent(Node *parent)
    {
        if (parent != this)
        {
            mpParent = parent;

            if (parent)
            {
                mpRoot = parent->getRoot();
            }
            else
            {
                mpRoot = this;
            }

            mParentNotified = false;
            needUpdate();
        }
    }

    void Node::_setDerivedPosition(const Vector3 &v)
    {
        setPosition(mpParent->convertWorldToLocalPosition(v));
    }

    const Vector3& Node::_getDerivedPosition() const
    {
        _updateFromParent();
        return mDerivedPosition;
    }

    void Node::_setDerivedOrientation(const Quaternion &q)
    {
        setOrientation(mpParent->convertWorldToLocalOrientation(q));
    }

    const Quaternion& Node::_getDerivedOrientation() const
    {
        _updateFromParent();
        return mDerivedOrientation;
    }

    const Vector3& Node::_getDerivedScale() const
    {
        _updateFromParent();
        return mDerivedScale;
    }

    void Node::_update(const bool updateChildren, const bool parentHasChanged)
    {
        mParentNotified = false;

        if (!updateChildren && !mNeedUpdate && !mNeedAllChildrenUpdate && !parentHasChanged)
        {
            return;
        }

        _updateFromParent(parentHasChanged);

        if (mNeedAllChildrenUpdate || parentHasChanged)
        {
            for (NodeSetIterator it=mChildren.begin(); it!=mChildren.end(); ++it)
            {
                Node *child = *it;
                child->_update(true, true);
            }
            mChildrenToUpdate.clear();
        }
        else
        {
            for (NodeSetIterator it=mChildrenToUpdate.begin(); it!=mChildrenToUpdate.end(); ++it)
            {
                Node *child = *it;
                child->_update(true, false);
            }
            mChildrenToUpdate.clear();
        }

        mNeedAllChildrenUpdate = false;
    }

    void Node::_updateFromParent(const bool parentHasChanged) const
    {
        if (mNeedUpdate || parentHasChanged)
        {
            if (mpParent)
            {
                const Quaternion &qder = mpParent->_getDerivedOrientation();
                mDerivedOrientation = qder * mOrientation;

                const Vector3 &sder = mpParent->_getDerivedScale();
                mDerivedScale = sder * mScale;

                mDerivedPosition = qder * (sder * mPosition);
                mDerivedPosition += mpParent->_getDerivedPosition();
            }
            else
            {
                mDerivedPosition = mPosition;
                mDerivedOrientation = mOrientation;
                mDerivedScale = mScale;
            }
            mNeedUpdate = false;
            mNeedTransformMatrixUpdate = true;
            mNeedInverseTransformMatrixUpdate = true;
        }
    }

    void Node::_updateTransformMatrix() const
    {
        if (mNeedTransformMatrixUpdate)
        {
            mTransformMatrix.makeTransformMatrix(
                _getDerivedPosition(),
                _getDerivedOrientation(),
                _getDerivedScale());
            mNeedTransformMatrixUpdate = false;
        }
    }

    void Node::_updateInverseTransformMatrix() const
    {
        if (mNeedInverseTransformMatrixUpdate)
        {
            mInverseTransformMatrix.makeInverseTransformMatrix(
                _getDerivedPosition(),
                _getDerivedOrientation(),
                _getDerivedScale());
            mNeedInverseTransformMatrixUpdate = false;
        }
    }
}

